#include "Motor.hpp"
/*
 * Foc.c
 *
 *  Created on: 2022年5月19日
 *      Author: Bing
 */

// extern volatile float ulTimerCounts;
float voltage_power_supply = 24;
float _normalizeAngle(float angle)
{
    float a = fmodf(angle, _2PI); // fmod()对浮点数取模、、
    return a >= 0 ? a : (a + _2PI);
}

// FOC核心函数：输入Ud、Uq和电角度，输出PWM
// PIDControl pid_iq(0.5f,0.01f,0,5,10);
// PIDControl pid_id(0.5f,0.01f,0,5,10);
extern volatile float iq;
extern volatile float id;
extern volatile float engle;
extern Encoder *encoder;
int32_t GetRawWhell(uint8_t dir)
{
    uint32_t temp = encoder->getRawEncoder();
    if (dir)
    {
        return temp;
    }
    else
    {
        return -temp;
    }
}
float GetAngle(void)
{
    return engle;
}

void SetAngle(float angle)
{
    engle = angle;
}
void Setdq(float uq, float ud)
{
    iq = uq;
    id = ud;
}

void setPhashVoltage_t(float Uq, float Ud, float engle_el)
{
    float Uout;
    uint32_t sector;
    float T0, T1, T2;
    float Ta, Tb, Tc;
    if (Ud) // only if Ud and Uq set
    {       // _sqrt is an approx of sqrt (3-4% error)
        Uout = sqrtf(Ud * Ud + Uq * Uq) / voltage_power_supply;
        // angle normalisation in between 0 and 2pi
        // only necessary if using _sin and _cos - approximation functions
        engle_el = _normalizeAngle(engle_el + atan2f(Uq, Ud));
    }
    else
    { // only Uq available - no need for atan2 and sqrt
        Uout = Uq / voltage_power_supply;
        // angle normalisation in between 0 and 2pi
        // only necessary if using _sin and _cos - approximation functions
        engle_el = _normalizeAngle(engle_el + _PI_2);
    }
    if (Uout > 0.577)
        Uout = 0.577;
    if (Uout < -0.577)
        Uout = -0.577;

    sector = (engle_el / _PI_3) + 1;
    T1 = _SQRT3 * sinf(sector * _PI_3 - engle_el) * Uout;
    T2 = _SQRT3 * sinf(engle_el - (sector - 1.0) * _PI_3) * Uout;
    T0 = (1 - T1 - T2) / 2;
    switch (sector)
    {
    case 1:
        Ta = T1 + T2 + T0;
        Tb = T2 + T0;
        Tc = T0;

        break;
    case 2:
        Ta = T1 + T0;
        Tb = T1 + T2 + T0;
        Tc = T0;

        break;
    case 3:
        Ta = T0;
        Tb = T1 + T2 + T0;
        Tc = T2 + T0;

        break;
    case 4:
        Ta = T0;
        Tb = T1 + T0;
        Tc = T1 + T2 + T0;

        break;
    case 5:
        Ta = T2 + T0;
        Tb = T0;
        Tc = T1 + T2 + T0;

        break;
    case 6:
        Ta = T1 + T2 + T0;
        Tb = T0;
        Tc = T1 + T0;

        break;
    default: // possible error state
        Ta = 0;
        Tb = 0;
        Tc = 0;
    }
    setPwm(Ta * PWM_Period, Tb * PWM_Period, Tc * PWM_Period);
}

void setPhaseVoltage(float Uq, float Ud, float angle_el)
{
    float Uref;
    float U_alpha, U_beta;
    float T0, T1, T2;
    float Ta, Tb, Tc;
    float angle_cos, angle_sin;
    uint8_t sector;
    angle_cos = cosf(angle_el);
    angle_sin = sinf(angle_el);
    //    angle_el = _normalizeAngle(angle_el);
    U_beta = Uq * angle_cos + Ud * angle_sin; //反park变换
    U_alpha = Ud * angle_cos - Uq * angle_sin;
    Uref = sqrtf(U_alpha * U_alpha + U_beta * U_beta) / voltage_power_supply; //计算参考电压矢量的幅值
    if (Uref > 0.57735f)
        Uref = 0.57735f; //六边形的内切圆(SVPWM最大不失真旋转电压矢量赋值)根号3/3
    if (Uref < -0.57735f)
        Uref = -0.57735f; //判断参考电压矢量所在扇区：
    sector = (angle_el / _PI_3) + 1;
    T1 = _SQRT3 * sinf(sector * _PI_3 - angle_el) * Uref; //计算两个相邻电压矢量作用时间
    T2 = _SQRT3 * sinf(angle_el - (sector - 1.0) * _PI_3) * Uref;
    T0 = (1 - T1 - T2) / 2; //零矢量作用时间
    switch (sector)
    {
    case 1:
        Ta = T1 + T2 + T0;
        Tb = T2 + T0;
        Tc = T0;

        break;
    case 2:
        Ta = T1 + T0;
        Tb = T1 + T2 + T0;
        Tc = T0;

        break;
    case 3:
        Ta = T0;
        Tb = T1 + T2 + T0;
        Tc = T2 + T0;

        break;
    case 4:
        Ta = T0;
        Tb = T1 + T0;
        Tc = T1 + T2 + T0;

        break;
    case 5:
        Ta = T2 + T0;
        Tb = T0;
        Tc = T1 + T2 + T0;

        break;
    case 6:
        Ta = T1 + T2 + T0;
        Tb = T0;
        Tc = T1 + T0;

        break;
    default: // possible error state
        Ta = 0;
        Tb = 0;
        Tc = 0;
    }
    // TIM8->CH1CVR = Tc * PWM_Period;
    // TIM8->CH2CVR = Tb * PWM_Period;
    // TIM8->CH3CVR = Ta * PWM_Period;
    setPwm(Ta * PWM_Period, Tb * PWM_Period, Tc * PWM_Period);
}
uint32_t Start_Flist[2] = {0};
volatile uint8_t flag = 0;
extern uint16_t cc_irq;
extern volatile int16_t Cir;
extern void OSDelay(uint16_t time_ms);
volatile uint8_t motor_start_flag = 1;
uint8_t Calibrate(uint16_t timeout)
{
    Flash *flash = new Flash(FALSH_CALIBRATE_ADDR);
    flash->ReadConfig((uint32_t *)&Start_Flist, 4);
    if (Start_Flist[0] != 0xf0f0)
    {
        for (int i = 0; i < 1000; i++)
        {
            OSDelay(1);
        }
#ifdef CH32
        TIM_ITConfig(TIM8, TIM_IT_Update, DISABLE);
        setPhaseVoltage(0, 4, 0);
        TIM1->CNT = 0;
#endif
#ifdef STM32
        setPhaseVoltage(0, 4, 0);
        TIM1->CNT = 0;
#endif
#ifdef AT32
        setPhaseVoltage(0, 4, 0);
        TMR1->cval = 0;
#endif

        encoder->setMode(1);
        Setdq(2, 0);
        Start_SVM_IT();
        encoder->setSpeed(0.001f);
        while (flag == 0)
        {
            timeout--;
            OSDelay(1);
            if (timeout <= 0)
            {
                encoder->setMode(0);
                Setdq(0, 0);
                motor_start_flag = 0;
                delete flash;
                return 0;
            }
        }
        flag = 0;
        Setdq(0, 0);
        encoder->setMode(0);
        Cir = 0;
        Start_Flist[0] = 0xf0f0;
        Start_Flist[1] = cc_irq;
        flash->SaveConfig((uint32_t *)&Start_Flist, 8);
        delete flash;
        return 1;
    }
    else
    {
        delete flash;
        encoder->setMode(1);
        Setdq(2, 0);
        Start_SVM_IT();
        encoder->setSpeed(-0.001f);
        while (flag == 0)
        {
            timeout--;
            OSDelay(1);
            if (timeout <= 0)
            {
                encoder->setMode(0);
                Setdq(0, 0);

                return 0;
            }
        }
        encoder->setSpeed(0.001f);
        OSDelay(200);
        flag = 0;
        Setdq(0, 0);
        encoder->setMode(0);
        Cir = 0;
// USB_Printf("%d, %d, %d \n",TIM1->CNT ,Start_Flist[1] ,cc_irq );
#ifdef CH32
        TIM1->CNT -= cc_irq - Start_Flist[1];
#endif
#ifdef STM32
        TIM1->CNT -= cc_irq - Start_Flist[1];
#endif
#ifdef AT32
        TMR1->cval -= cc_irq - Start_Flist[1];
#endif

        return 1;
    }
}

void Start_SVM_IT(void)
{
#ifdef CH32
    TIM1->CNT = 0;
    TIM_ITConfig(TIM1, TIM_IT_Update | TIM_IT_CC3, ENABLE);
    TIM_ClearITPendingBit(TIM1, TIM_IT_Update | TIM_IT_CC3);
    TIM_Cmd(TIM1, ENABLE);
    TIM_ITConfig(TIM8, TIM_IT_Update, ENABLE);
    TIM_ClearFlag(TIM8, TIM_IT_Update);
#endif
#ifdef STM32
    extern TIM_HandleTypeDef htim8;
    HAL_TIM_Base_Start_IT(&htim1);
    HAL_TIM_IC_Start_IT(&htim1, TIM_CHANNEL_3);
    HAL_TIM_Base_Start_IT(&htim8);
    TIM1->CNT = 0;
#endif
#ifdef AT32
    TMR1->cval = 0;
    tmr_counter_enable(TMR1, TRUE);
    tmr_interrupt_enable(TMR8, TMR_OVF_INT, TRUE);
    nvic_irq_enable(TMR8_OVF_TMR13_IRQn, 0, 0);
    tmr_flag_clear(TMR8, TMR_OVF_FLAG);
#endif
}

void setPwm(uint16_t Ta, uint16_t Tb, uint16_t Tc)
{
#ifdef CH32
    TIM8->CH3CVR = Ta;
    TIM8->CH2CVR = Tb;
    TIM8->CH1CVR = Tc;
#endif
#ifdef STM32
    TIM8->CCR3 = Ta;
    TIM8->CCR2 = Tb;
    TIM8->CCR1 = Tc;
#endif
#ifdef AT32
    TMR8->c3dt = Ta;
    TMR8->c2dt = Tb;
    TMR8->c1dt = Tc;
#endif
}

void ADC_Start(void)
{
#ifdef CH32
    ADC_SoftwareStartConvCmd(ADC1, ENABLE);
#endif
#ifdef AT32
    adc_enable(ADC1, TRUE);
#endif
}

Current Cur = {0};
float Car = 3.3f / 4096 / 0.5f;
Current *ADC_GetValue()
{
#ifdef CH32
    // Cur.Ia = (int16_t)ADC1->IDATAR3 * 0.001611328102F;
    // Cur.Ib = (int16_t)ADC1->IDATAR2 * 0.001611328102F;
    // Cur.Ic = (int16_t)ADC1->IDATAR1 * 0.001611328102F;
    Cur.Ia = (int16_t)ADC2->IDATAR3 * 0.0040283203125f;
    Cur.Ib = (int16_t)ADC2->IDATAR2 * 0.0040283203125f;
    Cur.Ic = (int16_t)ADC2->IDATAR1 * 0.0040283203125f;
#endif
#ifdef STM32
    // Cur.Ia = (int16_t)ADC1->JDR3 * 0.001611328102F;
    // Cur.Ib = (int16_t)ADC1->JDR2 * 0.001611328102F;
    // Cur.Ic = (int16_t)ADC1->JDR1 * 0.001611328102F;
    Cur.Ia = (int16_t)ADC1->JDR3 * 0.0040283203125f;
    Cur.Ib = (int16_t)ADC1->JDR2 * 0.0040283203125f;
    Cur.Ic = (int16_t)ADC1->JDR1 * 0.0040283203125f;
#endif
#ifdef AT32
    Cur.Ia = (int16_t)ADC1->pdt3 * 0.001611328102F;
    Cur.Ib = (int16_t)ADC1->pdt2 * 0.001611328102F;
    Cur.Ic = (int16_t)ADC1->pdt1 * 0.001611328102F;
    // Cur.Ia = (int16_t)ADC1->pdt3 * 0.0040283203125f;
    // Cur.Ib = (int16_t)ADC1->pdt2 * 0.0040283203125f;
    // Cur.Ic = (int16_t)ADC1->pdt1 * 0.0040283203125f;

#endif
    return &Cur;
}